package org.tacticalTroopers.jme.server;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import javax.vecmath.Matrix3f;
import javax.vecmath.Matrix4f;
import javax.vecmath.Quat4f;
import javax.vecmath.Vector3f;

import org.apache.commons.logging.Log;
import org.apache.commons.logging.LogFactory;
import org.tacticalTroopers.jme.server.object.ServerBullet;
import org.tacticalTroopers.jme.server.object.ServerElement;
import org.tacticalTroopers.jme.server.object.ServerGameArea;
import org.tacticalTroopers.jme.server.object.ServerGameMap;
import org.tacticalTroopers.jme.server.object.ServerPlayer;

import com.bulletphysics.collision.broadphase.AxisSweep3;
import com.bulletphysics.collision.dispatch.CollisionConfiguration;
import com.bulletphysics.collision.dispatch.CollisionDispatcher;
import com.bulletphysics.collision.dispatch.DefaultCollisionConfiguration;
import com.bulletphysics.collision.narrowphase.PersistentManifold;
import com.bulletphysics.collision.shapes.BoxShape;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.dynamics.DiscreteDynamicsWorld;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.RigidBodyConstructionInfo;
import com.bulletphysics.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
import com.bulletphysics.linearmath.DefaultMotionState;
import com.bulletphysics.linearmath.Transform;

public class Physicer {


	final static Log log = LogFactory.getLog(Physicer.class);

	Map<RigidBody,ServerElement> rigidBodyToServerElement = new HashMap<RigidBody,ServerElement>();
	Map<ServerElement,RigidBody> elementToRigidBody = new HashMap<ServerElement,RigidBody>();
	List<RigidBody> rigidBodies = new ArrayList<RigidBody>();

	//Map<Integer,Clientable> clients = new HashMap<Integer,Clientable>();
	DiscreteDynamicsWorld dynamicsWorld;
	//Map<Integer,ServerElement> dynamicServerElements = new HashMap<Integer,ServerElement>();

	List<RigidBody> staticBodies = new ArrayList<RigidBody>();
	//List<ServerElement> staticServerElements = new ArrayList<ServerElement>();


	//final CollisionShape boxShape = new BoxShape(new Vector3f(1f, 1f, 1f));
	//final CollisionShape sphereShape = new SphereShape(1f);

	boolean running = false;

	Map<Vector3f,CollisionShape> collisionShapes = new HashMap<Vector3f,CollisionShape>();

	List<ServerPlayer> players = new ArrayList<ServerPlayer>();

	ServerGameArea serverGameArea;
	public Physicer(ServerGameArea serverGameArea) {
		this.serverGameArea=serverGameArea; 
	}

	/*
    public synchronized void addClient(Client c, Vector3f position) {
	clients.put(c.getId(),c);
	RigidBody rb = createBody(1f, position,boxShape, 0f, null);
	dynamicBodies.put(c.getId(),rb);
	dynamicsWorld.addRigidBody(rb);

	ServerElement elem = new ServerElement();
	elem.setSize(new Vector3f(1.f, 1.f, 1.f));
	dynamicServerElements.put(c.getId(),elem);
    }
	 */


	public void init() {
		initPhysics();
		//addObstacle(new Vector3f(0f,0f,0f),new Vector3f(9f,0.1f,9f));
		//addObstacle(new Vector3f(-1.5f,1f,0f),new Vector3f(1f,1f,1f));


	}

	void initPhysics(){
		// collision configuration contains default setup for memory, collision
		// setup. Advanced users can create their own configuration.
		CollisionConfiguration collisionConfiguration = new DefaultCollisionConfiguration();

		// use the default collision dispatcher. For parallel processing you
		// can use a diffent dispatcher (see Extras/BulletMultiThreaded)
		CollisionDispatcher dispatcher = new CollisionDispatcher(
				collisionConfiguration);

		// the maximum size of the collision world. Make sure objects stay
		// within these boundaries
		// Don't make the world AABB size too large, it will harm simulation
		// quality and performance
		Vector3f worldAabbMin = new Vector3f(-100, -100, -100);
		Vector3f worldAabbMax = new Vector3f(100, 100, 100);
		int maxProxies = 1024;
		AxisSweep3 overlappingPairCache =
			new AxisSweep3(worldAabbMin, worldAabbMax, maxProxies);
		//BroadphaseInterface overlappingPairCache = new SimpleBroadphase(
		//		maxProxies);

		// the default constraint solver. For parallel processing you can use a
		// different solver (see Extras/BulletMultiThreaded)
		SequentialImpulseConstraintSolver solver = new SequentialImpulseConstraintSolver();

		dynamicsWorld = new DiscreteDynamicsWorld(
				dispatcher, overlappingPairCache, solver,
				collisionConfiguration);

		dynamicsWorld.setGravity(new Vector3f(0, -5, 0));
		


	}


	//Transform tmpTransform = new Transform();
	public void loadMap(ServerGameMap m) {
		List<ServerElement> elems = m.getStaticElements();
		for(ServerElement elem : elems){
			addStaticServerElement(elem);
		}
		/*Integer index = 0;
		for(RigidBody body : staticBodies){
			ServerElement element =  m.getStaticElements().get(index);
			if (body != null && body.getMotionState() != null) {
				body.getMotionState().getWorldTransform(tmpTransform);
				element.setTransform(tmpTransform);
			}
			index++;
		}*/
	}

	void addStaticServerElement(ServerElement elem) {
		RigidBody rb = createBody(0f,elem.getPosition(),getCollisionShape(elem.getSize()),0f, elem.getOriantation());
		staticBodies.add(rb);
		//staticServerElements.add(elem);
		elementToRigidBody.put(elem, rb);
		rigidBodyToServerElement.put(rb,elem);
		dynamicsWorld.addRigidBody(rb);	
		
	}


	public void addDynamicServerElement(ServerElement elem){
		addDynamicServerElement(elem,0f);
	}

	public void addDynamicServerElement(ServerElement elem,Float initialVelocity){
		RigidBody rb = createBody(elem.getMass(),elem.getPosition(),getCollisionShape(elem.getSize()),initialVelocity, elem.getOriantation());
		dynamicsWorld.addRigidBody(rb);	

		rigidBodyToServerElement.put(rb,elem);
		elementToRigidBody.put(elem,rb);
		rigidBodies.add(rb);
		//dynamicServerElements.put(elem.getId(),elem);
		if(elem instanceof ServerPlayer){
			ServerPlayer p = (ServerPlayer)elem;
			players.add(p);
			rb.setAngularFactor(0.3f);
		}

		if(elem instanceof ServerBullet){
			rb.setGravity(new Vector3f(0,-0.2f,0));
		}
	}


	public void removeServerElement(ServerElement element) {
		RigidBody rb = elementToRigidBody.get(element);
		if(rb==null){
			log.error("element "+element+" alredy removed");
		}else{
			rigidBodyToServerElement.remove(rb);
			elementToRigidBody.remove(element);
			rigidBodies.remove(rb);
			//dynamicServerElements.remove(element.getId());
			dynamicsWorld.removeRigidBody(rb);

			if(element instanceof ServerPlayer){
				ServerPlayer p = (ServerPlayer)element;
				players.remove(p);

			}
		}

	}

	CollisionShape getCollisionShape(Vector3f size) {
		/*CollisionShape cs = collisionShapes.get(size);
		if(cs==null){
			cs = new BoxShape(size);
			//cs = new SphereShape(size.x);
			collisionShapes.put(size,cs);
		}
		return cs;*/
		return new BoxShape(size);
	}



	/*
    public void start() {
	initStaticServerElements();
	for(ServerPlayer p : players){
	    p.getClient().setStaticServerElements(staticServerElements);
	}

    }*/



	Vector3f initialVelocityVector = new Vector3f();
	RigidBody createBody(float mass, Vector3f position,CollisionShape shape, Float initialVelocity, Matrix3f orientation){

		// Create Dynamic Objects
		Transform startTransform = new Transform();
		startTransform.setIdentity();
		// rigidbody is dynamic if and only if mass is non zero,
		// otherwise static
		boolean isDynamic = (mass != 0f);
		Vector3f localInertia = new Vector3f(0, 0, 0);
		if (isDynamic) {
			shape.calculateLocalInertia(mass, localInertia);
		}
		if(orientation!=null){
			startTransform.set(orientation);
		}
		startTransform.origin.set(position);
		// using motionstate is recommended, it provides
		// interpolation capabilities, and only synchronizes
		// 'active' objects
		DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
		RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(
				mass, myMotionState, shape, localInertia);

		RigidBody rb = new RigidBody(rbInfo);
		rb.setFriction(0.4f);
		//rb.setDamping(0.7f, 0.7f);
		if(initialVelocity!=0f&&orientation!=null){
			synchronized(initialVelocityVector){
				initialVelocityVector.set(0f,0f,initialVelocity);
				orientation.transform(initialVelocityVector);
				rb.setLinearVelocity(initialVelocityVector);
			}
		}
		return rb;


	}


	//Vector3f turnVector = new Vector3f(0f,2f,0f);
	//Vector3f turnInvVector = new Vector3f(0f,-2f,0f);
	public synchronized void step(Integer milliseconds){

		Boolean commanded;
		for(ServerPlayer p : players){
			commanded = false;
			if(p.getAccel()!=0f){
				setServerElementAccel(p,p.getAccel());
				p.setAccel(0f);
				commanded=true;
			}

			if(p.getStraf()!=0f){
				setServerElementStraf(p,p.getStraf());
				p.setStraf(0f);
				commanded=true;
			}

			if(p.getTurn()!=0f){
				setElementRotation(p,p.getTurn());
				p.setTurn(0f);
				commanded=true;
			}
			
			if(commanded){
				
			}else{
				setServerElementForce(p);
			}
			
		}

		updateRigidBodys();

		//dynamicsWorld.stepSimulation(1.f / 30.f, 10);
		dynamicsWorld.stepSimulation(milliseconds, 2);


		updateServerElements();

	}


	private void updateRigidBodys() {
		for(RigidBody rb : rigidBodies){
			ServerElement element =  rigidBodyToServerElement.get(rb);
			if (rb != null && rb.getMotionState() != null) {
				if(element.isUpdatedByUser()){
					
					/*rb.getMotionState().getWorldTransform(tmpTrans);
					tmpTrans.origin.x=element.getTransform().origin.x;
					tmpTrans.origin.y=element.getTransform().origin.y;
					tmpTrans.origin.z=element.getTransform().origin.z;
					*/
					rb.getMotionState().setWorldTransform(element.getTransform());
					rb.setMotionState(rb.getMotionState());
					element.resetUpdatedByUser();
				}

			}
		}
	}


	Transform tmpTrans = new Transform();
	void updateServerElements() {
		for(RigidBody rb : rigidBodies){
			ServerElement element =  rigidBodyToServerElement.get(rb);
			if (rb != null && rb.getMotionState() != null) {
				rb.getMotionState().getWorldTransform(tmpTrans);				
				element.setTransform(tmpTrans);
			}
			if(!rb.isActive()){
				rb.activate();
			}
			//log.debug(" "+element.getPosition());
		}

		for(Integer id : serverGameArea.getGameState().getDynamicElements().keySet()){
			ServerElement elem = serverGameArea.getGameState().getDynamicElements().get(id);
			elem.getCollidees().clear();
		}



		Integer nbManifold = dynamicsWorld.getDispatcher().getNumManifolds();
		for(Integer i=0;i<nbManifold;i++){
			PersistentManifold pm = dynamicsWorld.getDispatcher().getManifoldByIndexInternal(i);
			RigidBody rb0 = (RigidBody)pm.getBody0();
			RigidBody rb1 = (RigidBody)pm.getBody1();

			ServerElement elem0 = rigidBodyToServerElement.get(rb0);
			ServerElement elem1 = rigidBodyToServerElement.get(rb1);

			if((elem0 instanceof ServerBullet)||(elem1 instanceof ServerBullet)){
				log.info("coll "+elem0+" "+elem1);
			}


			elem0.addCollidee(elem1);
			elem1.addCollidee(elem0);


		}
 

	}
	Vector3f directionVector = new Vector3f();
	public void setServerElementForce(ServerElement element){
		//log.debug("accel");	
		synchronized (directionVector) {
			RigidBody rb = elementToRigidBody.get(element);
			Vector3f velocity = new Vector3f();
			rb.getLinearVelocity(velocity);
			velocity.scale(-1f);
			rb.applyCentralForce(velocity);
			
			Vector3f angularVelocity = new Vector3f();
			rb.getAngularVelocity(angularVelocity);
			angularVelocity.scale(-0.5f);
			rb.applyTorque(angularVelocity);
			//rb.setLinearVelocity(directionVector);
			
		}
	}
	
	public void setServerElementAccel(ServerElement element,Float accel){
		//log.debug("accel");	
		synchronized (directionVector) {
			setForwardDirectionVector(element);
			directionVector.scale(accel);
			RigidBody rb = elementToRigidBody.get(element);
			rb.applyCentralForce(directionVector);
			//rb.setLinearVelocity(directionVector);
			
		}
	}

	public void setServerElementStraf(ServerElement element,Float straf){
		//log.debug("accel");	
		synchronized (directionVector) {
			setRightDirectionVector(element);
			directionVector.scale(straf);
			RigidBody rb = elementToRigidBody.get(element);
			rb.applyCentralForce(directionVector);
			//rb.setLinearVelocity(directionVector);
		}
	}

	Quat4f oriantation = new Quat4f();
	Matrix4f rot = new Matrix4f();
	void setDirectionVector(ServerElement element,Float x,Float y , Float z){	
		RigidBody rb = elementToRigidBody.get(element);
		rb.getOrientation(oriantation);
		rot.set(oriantation);
		directionVector.set(x,y,z);
		rot.transform(directionVector);
	}
	void setForwardDirectionVector(ServerElement element){
		setDirectionVector(element,0f,0f,1f);
	}
	void setRightDirectionVector(ServerElement element){
		setDirectionVector(element,1f,0f,0f);
	}

	Vector3f turnVector = new Vector3f(0f,0f,0f);
	public void setElementRotation(ServerElement element,Float rot){
		//log.debug("rot");
		RigidBody rb = elementToRigidBody.get(element);
		synchronized (turnVector) {
			turnVector.set(0f,rot,0f);
			rb.applyTorque(turnVector);
		    //rb.setAngularVelocity(turnVector);
		}
	}






}
